clear;clc;      
delete(instrfindall)%������� 
global p1 p2 p3;
global P;
global r1 r2 r3;
x3=0;
y3=600;
z3=0;  %��Ϊ���㣨��ͷ������
p1=[x3-650 y3-600 z3];
p2=[x3+650 y3-600 z3];
p3=[x3 y3 z3];          %p3�ǻ�ͷ��p1������p2������
%
%% ��������
g=serial('com8');                                                          %�������ڶ���
set(g,'BaudRate',115200,'StopBits',1,'Parity','none','DataBits',8,'InputBufferSize',8000);
fopen(g);

%% ��ȡ��������
P = [10,10,10];
T = [10,10,5];
for i=1:1:10
    D=[];
    k=0;
while k<=200
a = fscanf(g,'%s',1000);
D =[D,a];           %��ȡ�������ַ���
k=k+1;
end
%disp(D);
disp('Another Time');
[r1,r2,r3]=dispose(D);   %�������ݵõ�������Ϣ
%RESULT=measure_result();

%% ���ռ������
%P0 =[100,100,100];
P0 = P;
options=optimset('MaxFunEvals',1e20,'TolFun',1e-18,'display','off');
P = fsolve(@root_p,P0,options)
%disp(P);
hold on;
plot3(P(1),P(2),P(3),'o');
%axis(-10000,10000,-10000,10000,-5000,5000);

%% ���ռ����ƽ���ϵ�ͶӰ��
fun2 = @root_t;
T0 = T;
options=optimset('MaxFunEvals',1e15,'TolFun',1e-18,'display','off');
T = fsolve(fun2,T0,options)
%disp(T);

%% �����

%% ���㴹ֱƫ��
mid=(p1+p2)/2;    %�ױ��е�
V=P(1,:)-mid;       %�ױ��е㵽��ͷ������       
N=cross(p3-p1,p3-p2);   % N��Ϊƽ�淨����
ver=dot(N,V)/(norm(N)*norm(V));  %�н�����ֵ
Ver = acos(ver);         
Ver_theta = double(180*Ver/pi);

if (Ver_theta >= 90)
    Ver_theta_d =Ver_theta-90;
else
    Ver_theta_d = 90-Ver_theta;
end
%Ver_theta_d;   %��ֱ�����ϼн�

%% ��ˮƽƫ��

Fla=dot(T-mid,mid-p3)/(norm(T-mid)*norm(mid-p3));    %�ױ��е�ͷ��ͶӰ�����������ͷǰ���������н�����ֵ
Fla_theta = acos(Fla); 
Fla_theta_d = double(180*Fla_theta/pi);
%Fla_theta_d; %ˮƽ�����ϼн�

%% ��ʾ���
RESULT=[Ver_theta_d Fla_theta_d];
disp(RESULT);
end

fclose(g);                                                                 %�رմ���
%}



